#include <opencv2/opencv.hpp>
#include <math.h>

using namespace std;
using namespace cv;

// 检查旋转矩阵是否可用
//  Checks if a matrix is a valid rotation matrix.
bool isRotationMatrix(Mat &R)
{
	Mat Rt;
	transpose(R, Rt);
	Mat shouldBeIdentity = Rt * R;
	Mat I = Mat::eye(3, 3, shouldBeIdentity.type());

	return norm(I, shouldBeIdentity) < 1e-6;
}
// 旋转矩阵转换成欧拉角
//  Calculates rotation matrix to euler angles
//  The result is the same as MATLAB except the order
//  of the euler angles ( x and z are swapped ).
Vec3f rotationMatrixToEulerAngles(Mat &R)
{

	assert(isRotationMatrix(R));

	float sy = sqrt(R.at<float>(0, 0) * R.at<float>(0, 0) + R.at<float>(1, 0) * R.at<float>(1, 0));

	// bool singular = sy < 1e-6; // If

	float x, y, z;
	// if (!singular)
	// {
	// 	x = atan2(R.at<float>(2, 1), R.at<float>(2, 2))/ CV_PI * 180;
	// 	y = atan2(-R.at<float>(2, 0), sy)/ CV_PI * 180;
	// 	z = atan2(R.at<float>(1, 0), R.at<float>(0, 0))/ CV_PI * 180;
	// }
	// else
	//{
	x = atan2(-R.at<float>(1, 2), R.at<float>(1, 1)) / CV_PI * 180;
	y = atan2(-R.at<float>(2, 0), sy) / CV_PI * 180;
	z = 0;
	//}
	return Vec3f(x, y, z);
}

int main(int argc, char **argv)
{
	Mat image = imread("//home//xwz/作业//lesson_5.jpg"); //*******图片路径记得修改*************/

	// 2D 特征点像素坐标，这里是用PS找出，也可以用鼠标事件画出特征点
	// 用电脑自带的画图软件   光标指向对应点  得到像素特征点

	// ***************这里是装甲板灯条的四个点****************
	vector<Point2d> image_points;
	image_points.push_back(Point2d(475,253 ));
	image_points.push_back(Point2d(474,350 ));
	image_points.push_back(Point2d(714,247 ));
	image_points.push_back(Point2d(720,340 ));
	// ****************用画图软件在测试图片中找到4个点坐标****************   顺序请参考下面的世界坐标顺序

	// 画出四个特征点  这4个特征点就是装甲板灯条4个点  像素坐标系
	for (int i = 0; i < image_points.size(); i++)
	{
		circle(image, image_points[i], 3, Scalar(0, 0, 255), -1);
	}
	// imshow("image",image);
	// waitKey(0);
	// return 0;

	// 3D 特征点世界坐标，与像素坐标对应，单位是mm
	std::vector<Point3d> model_points;
	model_points.push_back(Point3d(-66.75f, -24.25f, 0));
	model_points.push_back(Point3d(+66.75f, -24.25f, 0));
	model_points.push_back(Point3d(-66.75f, +24.25f, 0));
	model_points.push_back(Point3d(+66.75f, +24.25f, 0));
	// 　注意世界坐标和像素坐标要一一对应

	// 相机内参矩阵和畸变系数均由相机标定结果得出
	// 相机内参矩阵
	Mat camera_matrix = (Mat_<double>(3, 3) << 1.201371857055914e+03, 0, 7.494419594994199e+02,
						 0, 1.201435954410725e+03, 5.508546827593877e+02,
						 0, 0, 1);

	// 畸变系数
	Mat dist_coeffs = (Mat_<double>(5, 1) << -0.098380553375716, 0.006115203108383,
					   -4.766609631726518e-04, -0.001862163979558, 0);

	cout << "Camera Matrix: " << endl
		 << camera_matrix << endl
		 << endl;
	cout << "Distortion coefficient: " << endl
		 << dist_coeffs << endl;

	// 旋转向量
	Mat rotation_vector;
	// 平移向量
	Mat translation_vector;

	// pnp求解
	// 传3D特征点世界坐标 图像像素特征点 相机内参矩阵 畸变系数 传了两个空矩阵  一个存旋转向量 一个存平移向量
	solvePnP(model_points, image_points, camera_matrix, dist_coeffs,
			 rotation_vector, translation_vector, 0, SOLVEPNP_ITERATIVE);
	// 默认ITERATIVE方法，可尝试修改为EPNP（CV_EPNP）,P3P（CV_P3P）

	cout << "Rotation Vector " << endl
		 << rotation_vector << endl
		 << endl;
	cout << "Translation Vector" << endl
		 << translation_vector << endl
		 << endl;

	Mat Rvec;									// 接收旋转矩阵
	Mat_<float> Tvec;							// 接收平移矩阵
	rotation_vector.convertTo(Rvec, CV_32F);	// 旋转向量转换格式
	translation_vector.convertTo(Tvec, CV_32F); // 平移向量转换格式  //表面上似乎只是缩短了小数位

	cout << endl
		 << "After convertion:\nRotation Vector " << endl
		 << Rvec << endl
		 << "Translation Vector " << endl
		 << Tvec << endl;

	Mat_<float> rotMat(3, 3);
	// 旋转向量转成旋转矩阵
	Rodrigues(Rvec, rotMat); // 这个函数有两个作用  1.输入旋转向量，返回旋转矩阵 2.输入旋转矩阵返回旋转向量和雅可比矩阵
	cout << "rotMat" << endl
		 << rotMat << endl
		 << endl;
	// cout << rotationMatrixToEulerAngles(rotMat);

	float yawErr = atan(translation_vector.at<float>(0, 0) / translation_vector.at<float>(2, 0)) / CV_PI * 180;	  // 转换为角度
	float pitchErr = atan(translation_vector.at<float>(1, 0) / translation_vector.at<float>(2, 0)) / CV_PI * 180; // 转换为角度
	float yaw = atan(Tvec.at<float>(0, 0) / Tvec.at<float>(2, 0)) / CV_PI * 180;								  // 转换为角度
	float pitch = atan(Tvec.at<float>(1, 0) / Tvec.at<float>(2, 0)) / CV_PI * 180;								  // 转换为角度
	cout << "yawErr:\t" << yawErr << endl;
	cout << "pitchErr:\t" << pitchErr << endl;
	cout << "yaw:\t" << yaw << endl;
	cout << "pitch:\t" << pitch << endl;
	Mat P_oc;
	P_oc = -rotMat.inv() * Tvec; //.inv()是对矩阵求逆  对象必须为方阵
								 // 求解相机的世界坐标，得出p_oc的第三个元素即相机到物体的距离即深度信息，单位是mm
								 // while (true)

	cout << "P_oc" << endl
		 << P_oc << endl;

	imshow("Output", image);
	waitKey(0);
}
